

import java.util.ArrayList;
import lejos.nxt.*;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.TachoPilot;


public class BarCodeFSM extends Thread {
	
	static LightSensor sensor = new LightSensor(SensorPort.S1);
	static final TachoPilot pilot = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C, true);
	// samplingPeriod in milliseconds
	static final int samplingPeriod = 20;
	static final int numBufferSamples = 3;
	
	int[] signalBuffer;
	int[] barcode = new int[7];
	static ArrayList<Integer> barcodeLevels = new ArrayList<Integer>();
	static ArrayList<Integer> lightValues = new ArrayList<Integer>();
	int barcodeCounter;
	static boolean stopMe;
	
	boolean readingStrip;
	static int state;
	static int BROWN = 508;
	static int WHITE = 459;
	static int BLACK = 684;
	static int EDGE = 0;
	static int Vbz;
	static int Vwb;

	public BarCodeFSM() {
		// roep constructor van superclass aan ( Thread() )
		super();
		signalBuffer = new int[numBufferSamples];
		readingStrip = false;
		state = 0;
	}
	
	public static void main(String[] args)
	{	
		BarCodeFSM reader = new BarCodeFSM();
		BarCodeFSM.sensor.setFloodlight(true);
		
		lightValues.ensureCapacity(2000);
		
		System.out.println("Press ENTER to calibrate BLACK");
		Button.ENTER.waitForPressAndRelease();
		BLACK = SensorPort.S1.readRawValue();
		System.out.println(BLACK);
		
		System.out.println("Press ENTER to calibrate BROWN");
		Button.ENTER.waitForPressAndRelease();
		BROWN = SensorPort.S1.readRawValue();
		System.out.println(BROWN);
		
		System.out.println("Press ENTER to calibrate WHITE");
		Button.ENTER.waitForPressAndRelease();
		WHITE = SensorPort.S1.readRawValue();
		System.out.println(WHITE);
		
		Vbz = (BROWN + BLACK) / 2;
		Vwb = (WHITE + BROWN) / 2;
		
		System.out.println("Press ENTER to start driving");
		Button.ENTER.waitForPressAndRelease();
		LCD.clear();
		pilot.forward();
		reader.fillSignalBuffer(BROWN);
		reader.start();
		
		Button.ENTER.waitForPressAndRelease();
		stopMe = true;
		pilot.stop();
		LCD.clear();
		// prints the barcode on screen
		reader.interpretBarcode();
		System.out.println("Pilot stopped \n Press ENTER to connect");
		
		Button.ENTER.waitForPressAndRelease();
		connect();
		System.out.println("file stream closed");

	}

	public void run(){
		try {
			
			for (;;) {
				Thread.sleep(samplingPeriod);
				int lightValue = SensorPort.S1.readRawValue();	
				push(lightValue);
				int gradient = signalBuffer[signalBuffer.length - 1] - signalBuffer[signalBuffer.length - 2];
				
				boolean onEdge = Math.abs(gradient) >= 24;
				boolean lightBrown = (lightValue <= Vbz) && (lightValue > Vwb);
				boolean lightWhite = (lightValue <= Vwb);
				boolean lightBlack = (lightValue > Vbz);
				
				// switch statement werkt niet want BROWN,... zijn geen constanten
				if (state == BROWN) {
					if (onEdge) {
						readingStrip = true;
						state = EDGE;
					} else {
						state = BROWN;
					}
				} else if (state == WHITE) {
					if (onEdge) {
						state = EDGE;
					} else {
						state = WHITE;
					}
				} else if (state == BLACK) {
					if (onEdge) {
						state = EDGE;
					} else {
						state = BLACK;
					}
				} else if (state == EDGE) {
					if (!onEdge && lightBrown) {
						readingStrip = false;
						//interpretBarcode();
						// clean up the old ArrayList
						// barcodeLevels = new ArrayList<Integer>();
						state = BROWN;
					} else if (!onEdge && lightWhite) {
						state = WHITE;
					} else if (!onEdge && lightBlack) {
						state = BLACK;
					} else {
						state = EDGE;
					}
				}
				
				//testversie om barcodeLevels te bekijken op PC
				//barcodeLevels.add(state);
				lightValues.add(lightValue);
				//uiteindelijke versie om te interpreteren
				if(readingStrip)	barcodeLevels.add(state);
				if(stopMe)	break;
			}
			
		} catch (InterruptedException e) {
			LCD.clear();
			System.out.println("run() interrupted");
		}
	}
	
	private void push(int lightValue)
	    {
			for (int i = 0; i < signalBuffer.length-1; i++) {
				signalBuffer[i] = signalBuffer[i + 1];
			}
			signalBuffer[signalBuffer.length-1] = lightValue;
	    }

	private void interpretBarcode()
	{
		int barInterval = barcodeLevels.size() / 7;
		for (int j = 0; j < 7; j++) {
			float index = barInterval*j + barInterval/2;
			int intindex = Math.round(index);
			//barcode[j] = (barcodeLevels.get(intindex) < (WHITE + BLACK)/2) ? 1 : 0;
			int value = barcodeLevels.get(intindex);
			if(value == BLACK)
				barcode[j] = 1;
			else if(value == WHITE)
				barcode[j] = 0;
			else
				barcode[j] =2;
		}
		LCD.clear();
		System.out.println("The barcode is " + barcode[0] + barcode[1] + barcode[2] + barcode[3] + barcode[4] + barcode[5] + barcode[6]);
		System.out.println("barcodeLevels.size() is " + barcodeLevels.size());
	}
	
	public void fillSignalBuffer(int value)
	{
		for (int i = 0; i < signalBuffer.length; i++) {
			signalBuffer[i] = value;
		}
	}
	
	/**
	 * Estabish bluetooth connection to mission control
	 */
	public static void connect()
	{
		/*LCD.clear();
		LCD.drawString("Waiting", 0, 0);
		RConsole.openBluetooth(0);
		
		RConsole.println("=======================================LIGHT VALUES=======================================");
		RConsole.println("");
		for (int i = 0; i < lightValues.size(); i++) {
			String message = lightValues.get(i) + "";
			RConsole.println(message);
		}

		RConsole.close();*/
		
//		data = new File("lightLog.dat");
//		try {
//			data.createNewFile();
//			out = new FileOutputStream(data);
//		} catch(IOException e) {}
//		fileOut = new DataOutputStream(out);


	}
}